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Intelligent methods, functions and apparatus for load handling and transportation mobile 

robots. Patent application 

CROSS-REFERENCE TO RELATED APPLICATIONS 
This patent application claims the benefit of the prior Swedish patent application N:o 0300001- 
5, filed Jan. 2, 2003, for the same invention 

BACKGROUND OF THE INVENTION 
The invention relates generally to intelligent methods, functions and apparatus for mobile ro- 
bots in the form of autonomous vehicles and machines for load handling and transportation, 
based on laser-optic sensors for position determination and mapping of terrain, material volu- 
mes and other handling objects, obstacle detection and vehicle and implement control in loa- 
ding and unloading operations. 

Autonomous vehicles for handling and transportation of goods are widely used in indoor in- 
dustrial applications such as factory and warehouse logistics. 

Intelligent functions such as handling objects recognition and loading normally require palleti- 
sed or otherwise standardised packaging. There is a need for intelligent autonomous vehicle 
handling and transportation functions also for bulk material such as heavy chemical and mineral 
products in indoor as well as outdoor applications. 

For other outdoor applications such as large scale earthmoving in mining and excavation, there 
is an increasing demand for autonomous excavators, front shovels and the like. In this field, 
there exists solutions based on electro-optic sensors and satellite navigation systems for recog- 
nising and acquiring bulk material such as minerals, ores and gravel. 

In less extensive industrial applications, such as handling of solid fuels like biomass and coal 
and other products from forest and agriculture, there is a need for reliable, accurate, and cost 
effective technology. For these applications, the present technology for the extensive outdoor 
applications has some severe limitations. Many intelligent functions, such as navigation and 
handling object recognition and loading, are very dependent upon the continuous availability of 
accurate, real time, position determination information. Satellite position determination systems 
and its radio communication might get lost near buildings or through building walls. In order to 
back up satellite navigation, additional support technologies have to be installed on the vehicles 
and in the local environment such as inertial navigation systems and earthbound satellite 
transmitters, which leads to complicated and expensive system solutions at the risk of degraded 
reliability. The accuracy in position determination, particularly concerning the vertical "z" di- 
mension and the pitch and roll angles of the present outdoor earthmoving technology is also not 
sufficient for controlling loading operations where the z-coordinate of material volume and the 
underlying ground surfaces is of great importance. Thus there is a need for a more accurate, 
practical and simple single system solution for main autonomous vehicle intelligent functions 
such as work area and mission planning and control, navigation, material volume acquisition 
and obstacle detection and suitable for limited industrial sites that works well in outdoor as well 
as indoor or mining environments. 

The publication No. WO 87/02484 from 1987 mentions a driverless vehicle having a certain 
capability for autonomous loading and unloading of singular solid objects. The method requires 
each handling object to be provided with a number of reflectors. It is also required that the ob- 
jects are made in standardised measures. The method is not capable of bulk material loading or 
loading solid objects of varying and unknown shape. 



In US patent 5,548,516 from 1996 a driverless vehicle is described. This vehicle is capable of 
autonomous navigation based on GPS, inertial navigation and odometer based dead reckoning 
and it is equipped with a scanning laser rangefinder for obstacle detection and avoidance. The 
system has no dynamic terrain model and is lacking functions for autonomous material hand- 
ling. 

In a US research report:"Motion Planning for All-Terrain Vehicles: A Physical Modeling Ap- 
proach for Coping with Dynamic and Contact Interaction Constraints", IEEE Transactions on 
Robotics and Automation, Vol 15, No 2, April 1999, a path planning concept is presented for a 
mobile robot moving in unlimited terrain. The concept is based on full a pnon knowledge of 
the terrain topology, and problems related to material and load handling are not treated in this 
work. 

Neither is load handling and transportation treated in another research publication Autonomous 
Robot Navigation in Unknown Tenains: Incidental Learning and Environmental Exploration , 
IEEE Transactions on Systems, Man and Cybernetics, Vol 20, No 6, Nov/Dec 1990. This paper 
primarily deals with the problem of how to map the environment by means of a vehicle based 
terrain sensor. 

In US patent 5 974,352 a method is given for controlling a bucket by means of sensors for lift 
and tilt cylinder position and pressure, and how to optimize bucket lift and tilt movements ba- 
sed on integration offerees and movements in the load handling implement. The method requi- 
res an on board human operator for selecting loading point and for driving the vehicle to and 
from this point and who drives the vehicle in the entire loading movement and controls the 
bucket in the intial and final phases of the loading movement. 

In US patent 6,173,215 methods are given for autonomous navigation of a vehicle upon detec- 
ting an obstacle. Obstacle detections are only handled in a fixed to vehicle coordinate system 
and in real time and not recorded in a fixed to ground coordinate system in order to be conside- 
red in the forthcoming path. 

In US patent 6 223 1 10 Bl, a software architecture is given for autonomous earthmoving ma- 
chinery The described application is centered around digging and excavation, and applications 
such as wheel loaders are only mentioned but not dealt with in any further detail.There is no 
reference to any work concerning how to solve the position determination problems especially 
concerning the z-coordinate in limited industrial environments, or how to measure, with satis- 
factory accuracy, the shape, volume and position of material volumes and other general hand- 
ling objects in such environments. The problem of how to efficiently load and unload with an 
autonomous vehicle, industrial products temporarily stored on surfaces that may not be honson- 
tal or how to organise work in a limited areas is thus not dealt with in the patent or in its refe- 
renced publications. 

Accordingly, the present invention is directed to overcome one or more of the problems set 
forth above. 

BRIEF SUMMARY OF THE INVENTION 

This invention solves some of the above presented problems, in autonomous vehicles and ma- 
chines with load handling implements and systems for autonomous navigation within limited 
sites by defining for such a site a number of well defined zones for loading, unloading and au- 



tonomous navigation, by means of establishing a dynamic terrain model for the worksite 
ncluTng aZ- defining eaeh type of zone, and, in addition, layers for reference ground sur- 
face a best estimate of total terrain surface and a developing terrain surface estimate by 
^ploying a simple and cost effective system with a few on board sensors for vehic e posiUon 
determination and terrain surface measurement, where the principal elements of tins system 
comprise a combination of a scanning laser rangefinder and an ^^t^ot^Zo- 
freedom laser-optic position determination system, where a specific DTM (dynamic terrain mo 
del) computer analyses current measurements with respect to existing model values and thereby 
fs abH detect and with high accuracy record and analyse the surface of te~at^al vo ; . 
lumes general handling objects and obstacles in the present position and predicted pa h of the 
vXcl'e where the position determination system determines the position of the vehicle and the 
IctnVng'aser rangefinder in three dimensions and six degrees of freedom in a fixed to ground 
con nate system and in addition provides position data for the steering control of the vehicle 
and also enables the continuous creation and updating of the above mentioned dynamic terrain 
model to be unambigously performed in a fixed to ground coordinate system and whereto a 
Tpec fi on board vehicle mission computer, based upon attack point and ^ding heigh profile 
data in a loading operation or bucket emptying point in an unloading operation from the DTM 
computer, this mission computer optimizes neccessary parameters for the vehicle s path j*d the 
^ handling implement's movements during approach, loading paths or unloading movements 
n an unloading operation, and where this mission computer also coordinates and demands ve- 
hicle ^th anJfmplement movements both in previously fully planned and therefore static paths 
and movements as well as in the dynamically planned paths and movements the P*™?^ 
which have been calculated in the mission computer based on the analyses in the DTM compu- 



ter. 



BRIEF DESCRIPTION OF THE SEVERAL VIEWS OF THE DRAWINGS 
FIG 1 • Concepts tree. Relations between the concepts of driverless and autonomous and, in 

addition, preplanned versus intelligent and subconcepts to the latter. 
FIG 2- Load handling and transportation autonomous vehicle 1 with load handling implement 
14 and bucket 142, scanning laser rangefinder 81 for measuring the position of points on ter- 
rain surface, material volumes and obstacles for providing input data to the on board dyna- 
mic terrain model. Position determination system 7 with onboard rotating laser-optic sensor 

71 and suitable groundbased reflectors 72. The figure displays, in a vertical section, a laser 
ray being reflected by a material volume 181 and measuring the position of point P located in 

front of the vehicle 1. , „ Qt <»nt 

FIG 3- Principles for a laser-optic position determination system according to Swedish patent 
nr 464 837 for determining, in a fixed to ground coordinate system 41 , the position of a ve- 
hicle 1 in three dimensions along with its heading, pitch and roll angles from angular measu- 
rements in a fixed to vehicle coordinate system 42 to a number of fixed to ground reflectors 

72 within range of the rotating laser-optic sensor 71. 

FIG 4- Determining the 3D position (X, Y, Z), in a fixed to ground coordinate system 41, of 
points on the surface of terrain, material volumes and obstacles, from an arbitrarily positio- 
ned and oriented vehicle 1 employing an on-board scanning laser rangefinder 81 measuring 
in a fixed to vehicle coordinate system 42. 

FIG 5- Terrain map, from a dynamic terrain model, representing a variable Z as a fiinction of 
horisontal coordinates X and Y in a fixed to ground coordinate system 

FIG 6- Area with borders drawn for a material volume 181 subject to be loaded from by the 
system and including an obstacle-free zone 191 and zones for reconmussance 192 and loa- 
ding 193 respectively, an in preplanned reconnaissance path 111, a dynamically planned ap- 
proach path 121 from a first switch point 1112 reconnaissance/approach path to a dynami- 



sance/approach path ,0 ^^"f^^X^ movements^ the 
S% ~^ rc =«ion backwards/forwards. Static .—pc-p-taJM*- 
reauired on board the vehicle, including vehicle 

• esssssasisarss ksmss? 

stx^S of freedom position updates from the position determination system > ^ computes 
paZ^sltc and in such cases sends the corresponding obstacle detecfon message 984 

. ^decontrol compnter Ml. based on the vehicle control data lists 971 from the mis- 
and the vehicle control computer, 



. ra dio to* 5 with radio H* terminal » - C^tr in" 

formation and position data from the vehicle to 
. opener ^« ^ missions 
* ST.££2E ^Z—, for manna, vehiCe steering e.g. remote.y v.a radro 

Unk- • u ™th 7 1 1 with an inital set of a pair of clotoids and a circular path 

m ^,rsrJ s rssss. - «-* a P a ir of ^ 

section model of the bneket 142 in a sequence of pos,t,ons at states k - 0, 1, 2, 3, ... 

£ vide' ~« ruling, layer 2 drawing showing profile of reference ground surface, 
"1st esLate, layer f drawing showing profile of cujxenf as 
rain, difference Z(l,n) - Z(2,n), drawing showing profile of height above reference surtace 
calculated from developing and reference estimates. ,„« t: „ 
FIG 1 * " FtadTo ground projection of vehicle 1 and its obstacle avotdance mappmg 195} ,n 
the dynamic terrain model. 

DETAILED DESCRIPTION OF THE INVENTION 

"quel the short notation of system is used for device equipment, method or procedure or 
a combination of device or equipment and method or procedure. 

„i \ n ftPt 1 and its exDlanation below the invention concerns 

obstacles. 

The expression remote control is used for a system where a vehicle more or less continuously is 
IS^S^^Wtor via a communication medium, normally such as a radio link while 
ph^cally S Kcle and its working site or possibly by using a video link from a came- 
ra on boa^d the Vehicle but where such operator is working outside the vehicle. 



D 

operation without outside control It ^ » """"""""J ^ J^ded in exceptional and 

le can still be called autonomous. 

real time in order to meet changes and unexpected situations. 

Tfce word indigent is employed for a ^^^ZS^^^ <°< 
of available resources for the purpose of the system. 

Description of the invention 16 ^ 17 the invention concerns procedures in the 

3 fa and machines employed in loading, unlo- 

Ion with autonomous emergency action. The invents composes thereby, 

procedures for mapping a work site and its terrain surface comprising general handling objects 
180, material volumes 181 and obstacles 182 by means of; 

nn board vehicle 1 sensors where such a vehicle is provided with a position determination sys- 
tem 7 " t wd vehicle rotating laser-optic sensor 71 for accurate positron de « 
Ln onhe vehicle in three dimensions X, Y and Z in a fixed to ground ^^Za 
Z in addition heading, pitch and roll angle, by utilising fixed to ground reflectors 72 and, 

a svstem 8 for measuring, modeling and analysing terrain, material volumes and obstacles whe- 
re thtTystem comprises a scanning laser rangefinder 81 and a dynamic terrain mode 821 
HTM murrain model- or DTM-computer 82 for the purpose and provided with algorithms 
f J m^g - Treco ding terrain surface, material volumes and obstacles and, based on ^this 
durino aSon more or less continuously collected information and pnor mappings of the 
^Z^lorfi-g and unloading of material inside for the purpose 
ontimize coordinates for nearest or otherwise most optimum attack point 1222 for loaam g 
3^^S^fon«ml edge of the loading bucket 142 can begin its penetration of the 
ntl^lol^L to estimate, lor the purpose ^^^^^^ 
14, a height profile for the material volume along the intended path of the bucket during me 



and; 



ven from a; 



+ * «,hirh for the execution of a specific mission has been provided, by radio 
eranr „ for the superficia. veWcJe ^ = .-J— J » £E - «~ 

^ "J^S^^SSL.*. and action parameters /P5 for 
list to; 

SI operator station, halts the vehicle and alarms for operator intervention. 

With procedures in the form of intelligent functions for autonomous vehicles ; and ^machi- 
nes baled on laser-optic sensors for position determination and mapping 
ral Sng objects, material volumes and obstacles and vehicle and implement control 
he vThicle 1 is provided with a load handling implement 14 which includes a bucke 142 
or ote contrXble implement for handling and carrying loads. For planning, monitoring 
Ltte^nTng Z vehicle's operations there is an operator station 3 with man/machine 
mVerfece (MM?) computer, 31, being provided with radio link 5 for communication with 
2 ^Se s mission computer 6. The MMI computer can be provided with a number of 
nrenlanued mission instructions 9 each comprising one for each mission specific mission 
^Sl^ls^buting tasks and assignments to various subsystems on board and for 
Sing during missioS execution, that these tasks and assignments are earned out ~- 
coXg tomans L where this mission program returns the initiative to ^cg**** 
tion when the mission is completed or if it is by any other reason interrupted. In the mis- 
rnTnstruction there are also parameters 92 for static paths 11, prototype paramete s 930 
frdynamic paths 12, reconnaissance assignment 94 with, reconnaissance zone border 
polygon^ st Ld reconnaissance direction 941, loading zone, border polygon list and loa- 
SSrection 942, and unloading zone, border polygon list and unloading direction 943 
SfoSS detection assignment 95 with obstacle free zones 191, ^^ g onl^ 
951 and vehicle's obstacle detection geometries 195 and action parameters 952 In ^order to 
initiaTe such a mission, mission instructions 9 are sent from operator station 3 to the vehic- 
le's mission computer 6 via radio link 5. 
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Tn the total system there is a position determination system 7. This is employed for vehicle 

Wd veh,c e sub^tem 8 ^ obstacle V. The position determination 

Tm ue iverc xed ToTound coordinate system 41, vehicle position in six degrees 
rftedom Sng x y and ^coordinates and the three attitude angles y (headmg artgle), 
of freedom, being x y coordinate system the X-axis can 

l^^:»l^o^/™l in the horisoma. plane, ft. i Y-axis . ta 

V , « is also a right angular coordinate system, its ^axis can be defined to be 

SSTitS ahfng me full ,e„gth direction of the vehicle, me ,-ax.s 
orien eu in me vehicle's athwaer, direction and the <;-axis upwards and perpendtcularby to 
bodT S £axis and the n-axis. The fixed to vehicle coordinate system's position and on- 
enuJon mTpacTfs defined by the position (x,y,z) for its origo and Us rotations ,„ the 
fixed m ground coordinate system are defined by the three atttmde ^angles v (headmg 
annlel 3 (Ditch angle)and <p (roll angle). The angle V can now be defined as a clockwise 
* oiZ fiTed to vehic e coordinate system around its own <;-axis, as seen from a 
noTm on m fposidve par. of this C -axi, In the same way .he angle 9 is defined as a coun- 
L?lekwUe rottion of .he fixed to vehicle coordinale system around ,.s own n-axis as 
Sen tarn a POW on the positive par, of this n-axis, and the angle „ as a counter elockw,- 
r^ion ofT fixed ,c 'vehicle "coordinate sys.em around its own 5 -ax,s, as seen from a 
point on the positive part of this ^-axis, 

Such a oosition determination system 7 can be set up according to the procedures given in 
m lSS and which, in order to obtain six degrees of freedom in position 
Srforms angular measurements in azimuth and elevaUon to a number of 
fixed to ground reflectors 72. 

The oresent invention is a further development from this US patent through employment 
of a S i scanning laser rangefinder 81, for the purpose of with high precision 
dete»d meting the terrain, material volumes and other objects such as obstacles 
?n front Sfte vehicle, L its associated DTM-computer 82 for the 
based on the six degrees of freedom data from the position determination system 7, the 
S^Sro^ frl the scanning laser rangefinder into coordinates in the fixed to ground 
c^oTdTnaTe fstem 41 for points on the terrain surface, on material volumes 181 and on 
nccuS obstacles 182 and by these means build up and a dynamic terrain model DTM 
DTM various analyses can be performed enabling dynamic planning 
o?!he v^hk^s 1 movements such as the optimization of coordinates for target points and 
naXouts Tor hTvehicle's movement in loading and unloading tasks, and for optimiza- 
uon Sameters for movements of the vehicle's and its implements in such loading and 
£S« m addition, the DTM can be used for detecting, in S1 de obstacle free 
zones 191, any ocurring obstacles and to prompt suitable reactions. 

Furthermore the vehicle 1 is arranged for autonomous operation by being provided with a 
ve^cle^ontrol computer 211 with interfaces 212 to the vehicle's electrical system, ^indu- 
dLg engTne, gearbox, main brake, parking brake, and vehicle steering system, and interfa- 



ces to the load handling implement's 14 sensors and actuators ^interface ££££££ 
termination system 7, interface to the operator station 3 via radio link 5^ interface to the 
Son computer 6 and, for obstacle avoidance functions also an interface to the DTM- 

rnmnuter 82 with its digital terrain model 821. . 

XTlto actiM of the vehicle control comparer 211 is based on the veh.ele and .mple- 
^ZSZfmm whieh is dynamieally generated by the mission eomputer^ by 

It Hst fs ^ d ta Ta set of parameters 931 dynamically optimized by a program ,n 
rmirrpurer 6. Por this purpose P = for^tion of apP^mg 

compute 82 to the form of detection, loading path and nnloadmg pom, messages 981, 
982, and 983, respectively. 

In the DTM-computer 82 there is a dynamic terrain model DTM 821 w^-rs the en- 
tire work site with transport routes and zones for reconnaissance 192, loading 193 and 
l oi ^e DTM computer receives, continuously during the mission position 
da r^m the position determination system 7. By means of from the position determina- 
tion sys^btained coordinates and attitude angles, in the fixed to gr ound coordi nate 
iTrTfor a six degrees of freedom position of the fixed to vehicle coordinate system 
7 2 Z l^r^Xtl measurements are transformed from this fixed to .vehicle ■ co- 
ofdi^teZtr to positions in the fixed to ground coordinate system for updates of the 

computer 6 the DTM computer is provided whh suitable ) zone : bor- 
H«- li on lists 941 942 and 943 for reconnaissance 192, loading 193, and unloading 
f^^^^^^ovided in mission instruction 9. Based on these data a crite- 
ria is S the DTM computer to decide when the vehicle is inside reconnais- 
sTce z7ne wnVnce this computer commences to actively update, by using range and angle 
n^asu^emems from the scanning laser rangefinder 81, the DTM 821 inside the current lo- 
S or loading zone. During a loading or unloading operation a reconnaxss^ce pa h 
7 7 7 fs inserted after some initial static paths possibly required in order to get the vehicle 
ufficien ly near the current loading or unloading zone. During the reconnaissance path 
^Sels to detect a feasible point, the attack point 1222 on the material volume 
1X1 where the bucket 142 can start to penetrate during a dynamic loading path 122 with 
hf co": movements of vehicle and bucket or a feasible location, the bucket empty- 
ing point 1232 with its constituent movements of vehicle and bucket. 

In narallell with the above DTM 821 updating process the DTM computer 82 also analy- 
sed ; roevelopnTg part of the DTM as updated by the incoming measurements when ad- 
vancingTn tne reconnaissance path in order to find, in a loading operation, by means of a 
3 algorithm 824 for optimizing the attack point 1222 position, the 
herwLe most optimal attack point, or, in an unloading operation, by means of a specific 
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algorithm 826 for optimizing the bucket emptying point 1232 position, the ^most distant or 
olerwise most optimal point. As soon as the optima, coordinate * ° T 
1232 has been found the DTM computer sends, to the mission computer 6, a Qete ™ on 
message 981 with coordinates for the vehicle's 1 current position and coordmates for the 
r t mal attack or unloading point. In the mission computer can then be obtained parame- 
te'rs "of dy^am cTpproacnpath 121, by means of mission computer program 611 for ap- 
P^oach paToptimiition, bJLd on the arriving data from the DTM 
orototvue path parameters 930 in mission instruction 9. This dynamic approach path, as 

X 9311, vehicle and implement control data list 971, -d -tch Point 
reconnaissance/approach path 1112 for finishing the current reconnaissance padi 111 le- 
aTtrvehTcle to a suitable position, vehicle loading point 1221 representing ft* , posi ion 
of the vehicle in front of the attack point 1222 in a loading operation, or unloading point 
1231 ^representing the position of the vehicle in front of the bucket emptying point 1232 n 
1 unSng Operation The mission computer also sends a path switch message 972 with 
"oTdmaSor'the switch point and approach path vehicle and ^^^f 
to the vehicle control computer 211. The mission computer also sends a report point 1211 
message 973 to the DTM computer with coordinates for the point on the path where the 
StM computer shall deliver, to the mission computer in a loading operation a loading 
path message 982 with estimated coefficients 9821 for an analytic approximati on of the 
Sound surface at the vehicle loading point and a loading height profile data list 9822 with 
z-coord Z es for points on the material volume 181 surface along the loading direction 
Lm he attack point 1222, or in an unloading operation a vehicle unloading point mes- 
aTttS with estimated coefficients 9831 for an analytic approximation of the ground 
urfa!e at Ae unloading point and a vehicle unloading point local terrain model parameter 
U^9832 f* a local terrain model of the material volume around the bucket emptying po- 
int 1232 The DTM-computer analyses the developing DTM in order to perfect the coef- 
ficients and parameters required by the mission computer. When in a loading operation, 
he report po'int has been reached and the loading path message 982 has been sent over 
from die DTM computer the mission computer then optimizes the loading path 122 para- 
meters 9312 based on the recieved message data, and loading path prototype V™™*™ 
9302 in the mission instructions 9. This optimization is done by means of program 612 for 
loading path and its constituent bucket movement optimization. In an unloading opera- 
nd" the unloading point message 983 has been sent over from the DTM computer 
the mission computer then optimizes the unloading movement paramete rs 9313 based on 
the received message data, and unloading operation prototype parameters 9303 in the 
mission instructions 9. This is done by means of program 613 for unloading bucket mo- 
dernization. When in this way the valid parameters for loading or unloading ve- 
WcTe patiTand vehicle and implement movements are available, before actually being car- 
ried out these movements are simulated in the mission computer by means of its vehicle 
path Id load handling implement movement simulation algorithm 614 By this seda- 
tion a new vehicle and implement control data list is generated which then is sent to the 
veWcle control computer. This computer switches its control of the vehicle and rts imple- 
ments from the approach path to the loading path or unloading movement according to the 
rece ved <£ta upon arrival to the vehicle loading point 1221. Finally, the vehicle's advan- 
dfg movements are retarded and brought to finish, while the bucket 142 is allowed to 
perform its proper movements as planned for the loading or unloading operation in ques- 
Son When even these movements are finished, the vehicle control computer requests, for 
the return path 124 and its constituent bucket movements, a new vehicle and ^P^nt 
control data list from the mission computer. The mission computer then optimizes the re- 
turn movement parameters for a path to some point where the mission can be further con- 
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tinued according to the mission program 61, based on the actual poattm of vehicle and 
implements in the finished loading or unloading movements, and if from a loading opera- 
tion also based on prototype parameters 9304 for a dynamic return transport path 124. 

During all operations with an autonomous vehicle or machine 1 according to this invention, 
aZher duty for the DTM computer 82 is to compare currently received measurements from the 
^ng llr rangefinder 81 with the already available dynamic terrain model when m the ob- 

free zoneTpi, and also to continuously evaluate criteria for obstacle detection or any 
P os bfe fn^on of the vehicle or machine outside the obstacle free or loadmg or unloading 



areas. 



MM „nnp terrain co~ r™™ ^rence surface peneral handli ng ob jects material volumes and 

SSSning laser rangefinder 81 ought to be installed relatively high on the vehicle 1, cf FIG 
2 suchTon the forwatd edge of the vehicle's cabin roof, forward oriented and in addition 
Lhned'ownwaxds. The laser light is pulsed and its time of flight to reflection and back to the 
receiver is employed for the distance measurement. The laser rangefinder is scanning ; in ^azi- 
muth by means of a rotating mirror. This way, the laser rays are sent out in a plane paral ell to 
the n-axis of the fixed to vehicle coordinate system 42 but with a perpendicular to the plane 
tilted forwards in the %- £ - plane with an angle 0 from the C-axis. The laser .ays 
terrain in a sequence of points and as seen in a crossection in the point P (FIG 2). The scanning 
ir ^rangefinder 81 measures distance R to a reflection and angle a cf FIG 4 m the ^inclmed 
plane where this angle is defined of a) the abovementioned vehicle ^-axis and b) by the instan- 
taneous vector orientation of the laser ray. 

The coordinate vector X= & n, Q for me position in the fixed to vehicle coordinate system 
42 for each such point P can now be obtained by means of these measurements a and R the 
scanning laser rangefinder's 81 position (^o, fy) in the fixed to vehicle coordinate system and 
the scanning laser rangefinder tilt angle (3 from the i;-axis: 

£, = £,o + R cosacosfi ] 
r|=T)o + Rsina \ 
C ) = C > o + R cosasinp J 

With these coordinates in the fixed to vehicle coordinate system 42 and by employing _ the six 
degrees of freedom position data from the on board position determination system 7 the ^coordi- 
nates of the scanning laser rangefinder's measured point P can now be obtained in det fixed to 
™oZ ££2 system 41. The six degrees of freedom position determination from system 7 
comprises the X = (X, Y, Z) and the attitude angles vy, 8 and <p, the latter needed for the trans- 
formation matrix M(y, 9, cp): 

M(y, B, <p) = "i 
T cosm/cos», sinM/cos9, sind . 

= | -sinvj/coscp + cosM/sinSsincp, cos V cos<p + sinysinSsmq,, - cosSsmcp (2) 
L-sinysinq. - cosysin&coscp, cosvysimp - sirups inScoscp, cos&coscpJ 

The X. as = (xias, yas, Z.as) in the fixed to ground coordinate system 41 for the point P can now 
be obtained as: 

Xi as = X + X*M(M/,d,cp) ( 3 > 
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This way each measurement with the scanning laser rangefmder 81 results in a three dimension 
loo dTnSe determination in the fixed to ground coordinate system 41. The calculations of 
equations (1), (2) and (3) are made in the DTM-computer 82, and the obtained coordinates are 
fTcTparei with the current dynamic terrain model 821 in the DTM-computer, such as for 
detecting a new obstacle, and finally for uppdating the dynamic terrain model. 

Such a terrain model 821 can be based on a quadratic grid. With a square mesh side lengt of d = 
0 33 m an area of 100,000 m 2 is covered by roughly one million squares. If for each square is 
stored 32 byte index, Z-value, age of data and accuracy measure in the dynannc terrain model 
82 l a storage space if 32 Mbyte'is required in the DTM-computer 82 for this model allowing a 
resolution of some 1 /64000 in the Z variable. Let a square be identified as ( i, j ) when its four 
corners has the coordinates in plane X/Y: 

lower left corner: ( i- 1 , j- 1 )d (4a) 

lower right corner: ( i- 1 , j )<* ( 4b ) 

upper left corner: (ij-l)d ( 4c ) 

upper right corner: ( i, j )d ( 4d ) 

The centre point of a square identified as ( i, j ) has the coordinates in plane X/Y: 

fXi = (i - 0.5)d, 1 < i < imax (5a) 
lYj = G - 0.5)d, 1 < j < jmax (5b) 

Each measurement X.as can then be computed with or update the terrain model 821 in the 
square ( i, j ) where these indexes i and j are determined by the inequalities: 

f( i - 1 )d < xias < id ( 6a ) 
\ 

1 )d<yias< jd (6b) 

The dynamic terrain model DTM 821 is required to be defined in every point of an entire work 
site be it for comparisons with fresh measurements (obstacle detection), or be it ^ optimiza- 
tion of coordinates 9811 and 9812 for attack point 7222 or bucket emptying point 1232 and for 
optimizing loading path 122 and unloading movement 123 where such optimizations require 
estimations of loaded volume or available volume for unloading material for various sets of 
parameters for these movements. Some parts of the DTM can be prepared before a mission is 
executed while others are based or updated also in important layers from earlier measurement 
runs with vehicle and its sensors. To each square (i, j) in the DTM, 1 < i < imax and 1 < j < 
jmax an ordered sequence of numbers n can be defined in a way so that to each integer value n 
corresponds unambiguously a certain model square (i, j) and to each model square (i, j) cor- 
resonds the same nuber n: 



n 

or the inverse: 



fiimax+ i if jmax < imax 1 

\ } ( 7a > 

U + jjmax if imax < jmax J 

[integer part of fraction n/imax if jmax < imax 1 

\ ~ f (7b) 

U-j jmax if imax < jmax J 
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fn-Hmax if jmax < imax 1 

: = * r (7c) 

I integer part of fraction n/jmax if imax < jmax J 

Various layers can be employed in the DTM for separating different kinds of zones and data in 
the DTM. Cf FIG 16. The identification Z(LAG, n) represents Z-coordinate i layer LAG for 
element n in DTM. In the sequel is employed: 

Layer 0. Index number n 

Layer 1 Z(l , n) is a developing estimate based on measurements only from the vehicle s 
current running in the path for detecting and measuring the terrain surface including handling 
objects 180, material volumes 181, and obstacles 182. 

Layer 2. Z(2, n) represents the reference ground surface and is intended to be an actual best 
model estimation of the real ground surface without any handling objects, material volumes, 
or known and unknown obstacles. 

Layer 3. Z(3, n) is intended to be a current best model estimation of the total site terrain sur- 
face and represents handling objects, material volumes and known obstacles for which the Z- 
values of the model elements are determined from measurements from earlier paths and ope- 
rations on the site or otherwise provided fundamental data. It is also assumed that measure- 
ments from earlier unknown but after the detection physically removed obstacles have been 
cleaned away from those model squares once occupied by measurements from such currently 
removed obstacles. 

Layer 4. Obstacle-free zone 191 denotation field. Z(4, n) = 1 for obstacle free zone. 
Layer 5. Reconnaissance zone 192 denotation field. Z(5, n) = 1 for reconnaissance zone. 
Layer 6. Loading zone 193 denotation field. Z(6, n) = 1 for loading zone. 
Layer 7. Unloading zone 194 denotation field. Z(7, n) = 1 for unloading zone. 

Measurements concerning element n can be stored in several ways: 

. Last measured Z-value, written as Z-i (L, n), is stored. Once stored, the value is written as 
Zo(L, n) 

• A moving average from the last k measurements is stored and written also as Zo (L, n), that is 

Zo (L, n) =[ Z-i (L, n) + Z-2 (L, n) + ... + Z-k (L, n)] / k (8a) 

• A recursive filter with the experimental filter coefficient y, 0<y<l is employed for updating 
the model value Zo (L, n) with the value Z-i (L, n) from the last measurement. The filter can 
be initialised by: Zo (L, n) = Z-i (L, n) the first time a measurement represents element n, sub- 
sequently the following recursive filter equation can be employed: 

Zo (L, n) = yZo (L, n) + (1 -y)Z-i (L, n) (8b) 
This kind of method is suitable if a large number of measurements is expected for each element 



n. 



Criteria for DTM-based obstacle detection 

Autonomous driving is allowed in obstacle free zones 191 and in loading 193 and unloading 
zones 194. Obstacle free zones are only allowed outside loading and unloading zones. The ob- 
jective of the DTM-based obstacle detection function is to test for obstacle detection in element 
n the hypotesis that the criterion H < [Z(l ,n) - Z(3,n)] holds for a minimum obstacle height H 
for each element n where Z(4,n) = 1 and where this element is located inside or within a spe- 
cific neighbourhood of the vehicle in its current or planned and predicted position. Thus, if an 
obstacle is detected in element n of the DTM, Z(4,n) is set to zero until the obstacle is removed. 



14 



Employing the differences Z(l,n) - Z(3,n) has the benefit, especially in cases of uneven terrain 
and reference surface, to produce more accurate measurements of obstacles. Cf FIG 16. 

Creation and updating of material volume models 

As the vehicle on reconnaissance path 111 enters reconnaissance zone 192, cf FIGs 6 and 7, 
those measurements representing terrain model elements inside loading 193 or unloading 194 
zone are used to create and update the developing layer 1 of the DTM model inside such a zone 
representing the material volume model. The purpose is primarily to collect fresh data for the 
currently forthcoming approach path 121, loading path 122, or unloading movement 123. By 
storing model values to the next path or operation such a dynamic model can contain certain 
errors as the shape and size of the material volume 181 might have changed when it is going to 
be approached next time. On the other hand it would be of value if neighbouring loading or 
unloading zone elements not centrally involved in the previous run still have become updated in 
the model during such a run to be used for e.g. planning further reconnaissance paths in this 
area. 

Optimization of attack point 7222 and bucket emptying point 1232 coordinates 
Several factors have to be considered when optimizing attack point 7222_position in a loading 
operation. It is important to select such attack points so that the remaining material does not risk 
to interfere with the return movements from a loading path 722. For this reason strategies lea- 
ving an outward concave material volume front ought to be avoided even if such a strategy at 
first thought might appear attractive in order to minimize distance travelled. On the other hand a 
strategy producing an outward convex front would lead to unneccessarily long travel distances 
and require quite large areas for vehicle maneouvers. Therefore, a strategy leaving a straight 
front line is the natural choice. These factors are also applicable in unloading. This way, the 
material volume 181 can be attacked from one direction all the time thus considerably simplify- 
ing approach and exit path planning. 

A method to get such a straight front line is, to select attack point 1222 ox bucket emptying po- 
int 1232 on edges of the material volume 181 maximally deviating from such a straight front 
line. A straight line facing and parallell to the desired front line and located at some distance 
from the front can serve as reference. In a loading operation, attack points are chosen where the 
distance between the candidate point and this reference line is minimum. In an unloading ope- 
ration, bucket emptying points are chosen where this distance is maximum. For the DTM 821 
has been defined Z(6,n) = 1 to represent a situation where element n is in a loading zone 193 
and Z(7,n) = 1 means that element n is in an unloading zone 194. 

Threshold levels Hioad and Hunioad representing least volume height worth loading from and 
maximum volume height worth filling to, respectively, are employed in order to avoid that too 
small spill heaps, material rests und unevennesses would cause unnesseccary work load. 

Optimizing the coordinates of attack point and bucket emptying point 

a) based on nearest or most distant point in the material volume as measured from a reference 
line 

When the vehicle 1 in a loading operation, cf FIG 6, drives forward on reconnaissance path 777 
and begins approaching a material volume 181 in a loading zone 193 measurements will com- 
mence regarding elements of DTM 821 inside the loading zone with a height above threshold 
level for loading, provided there is material enough to load. In order to avoid, in situations with 
a multitude of elements in the DTM suitable for loading, that only the first element that satisfies 
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the threshold criterion will be selected, it is required that the vehicle continues a given further 
distance after this event. The endpoint on this travelling distance can be defined as detection 
point 7777. At this detection point it is possible that a multitude of elements have been recorded 
and can be employed for final selection of attack point 7222 for loading. If still only the first 
detected element has been recorded it is probably singular in a sufficiently large environment 
and can therefore be selected as attack point on good grounds in such a case. The detection 
event is defined as the moment when the vehicle arrives at this detection point. 

For an unloading operation, cf FIG 7, a similar reasoning leads to a procedure for selecting 
bucket emptying point 7252. A criterion for this point in unloading is that the vehicle 1 has mo- 
ved a given further distance since the first occurrence of measurements representing elements in 
the DTM 821 where the element volume is measured not to allow further unloading, alternati- 
vely that the entire surface is empty, thus allowing the reconnaissance path to be aborted when 
the most remote border of the unloading zone 194 has been passed, with required margins, by 
the measurements of the scanning laser rangefinder 81. The unloading can start at this most 
remote border. 

At the detection point 7777 the binary occupancy vectors Qioad(n) and Qunioad(n) can be defined 
as 

fO if [Z(l,n)-Z(2,n)]<Hioad 1 
Qioad(n) = \ \ (9a) 

ll ifHioad<[Z(l,n)-Z(2, n)] J 

fO ifHunioad<[Z(l,n)-Z(2,n)] 1 
Qunload(n)= { \ (9b) 

ll if [Z(l,n)-Z(2,n)] <Humoad J 

As attack point 7222 for loading and bucket emptying point 7252 at unloading can now be se- 
lected the coordinates for element n = nioad and n = nunioad when these points are most close and 
most distant, respectively, from a straight line, arbitrarily chosen to suit the purpose, cf "A- A" 
in FIGs 6 and 7, and where the elements nioad and nunioad, respectively, also satisfy the condi- 
tions: 

for loading, requirement for nioad 

fQload(nioad) =1 (10a) 
\ 

lZ(6, nioad) =1 (10b) 
for unloading, requirement for nunioad 

fQunload(nunload) =1 (H&) 

\ 

lZ(7, nunioad) =1 (lib) 
b) based on nearest or most distant point in a cell belonging to an ordered sequence: 



and 



In this alternative each element n satisfying Z(6,n)=l or Z(7,n) = 1 belongs to an ordered 
sequence N= 1,2,3,..., NMAX of cells, where we take T(N,n) = 1 to indicate that element n be- 
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longs to cell N. At loading and unloading, to the condition pairs (10a), (10b) and (11a), (1 lb), 
respectively, are added the further conditions: 

r(N,nioad)= 1 (10c) 

T(N, nunload) = 1 (He) 

where cell N is currently selected for loading/unloading. 

Path generation at entering and exiting from a loading or unloading zone 

A mission instruction 9 with loading or unloading operations has to be prepared with at least 
one preplanned static reconnaissance path 777, for the vehicle 1 to follow while its sensors are 
scanning and analysing the material volume 181 from or to which loading or unloading, respec- 
tively, is desired, cf FIGs 6 and 7. During the vehicle's travel along this reconnaissance path, 
coordinates for an attack point 7222 or for a bucket emptying point 7232 are obtained at the 
detection point 7777, as a result of the analysis in the DTM-computer 82 by means of the abo- 
vementioned conditions and criteria applied on the successively, during the vehicle's travel, 
developing Z(l, n), n = 1, 2, 3, ... of the dynamic terrain model 821. At the detection event as- 
defined above, the DTM-computer sends a detection message 981 to the mission computer 6 
with coordinates 9811 or 9812 for the selected attack point or bucket emptying point, respecti- 
vely. It is now possible for the mission computer to determine the position of a switch point 
7772 where the reconnaissance path can be finished and an approach path 727 can start, by 
knowing the amount of time required for the system to optimize, parameters 9311 for the dy- 
namic approach path 727 and produce and send vehicle and implement control data list 971 to 
the vehicle control computer 211. The approach path has to be designed to lead the vehicle to a 
position, the vehicle loading point 7227, the position of the vehicle 1 when the bucket 142 starts 
penetrating the material volume 181 at the attack point 7222 or, in an unloading operation to a 
position, the unloading point 7257, from which the vehicle can start some finishing maneouvers 
to be prepared for the unloading movement 723. The coordinates (Xc, Yc, Zc) for the vehicle's 
position in the vehicle loading point can be obtained from the coordinates of the attack point by 
knowing the geometry of the vehicle and its load handling implement 14 and the actual loading 
direction \|/c. Coordinates for the vehicle's position in a vehicle unloading point can be obtai- 
ned in an analogous way. 

As the selected loading or unloading point not always is located straight in front of the vehicle 
1, the dynamic approach path 727 has to allow for, on the travel from the switch point 7772, 
first a sideways translation of the vehicle, and second to guide the vehicle to the intended direc- 
tion in the vehicle loading 7227 or vehicle unloading point 7237. Thus a more or less s-shaped 
path is required. By employing, in path generation, a mix of clotoid, circular and straight path 
segments, where the clotoid segments will bring the path's radius continuously between straight 
path and least turning radius, a large variation of such paths can be designed in a manner which 
is contributing to limit steering errors. A simple two-dimensional path model of limited comp- 
lexity that works well for this purpose, cf FIG 9, consists at its maximum of two bends with a 
straight path between. In the first bend the heading angle is changing the amount Oti, and Oil in 
the second. Each bend consists of a pair of clotoids and, for big Oti and CC2, a circular path seg- 
ment inserted between bending and straightening clotoid. For a simple path without any obstac- 
le, where start point and end point are given as three-dimensional coordinate vectors Xa and Xc 
and heading angles \|/a and \|/c respectively, a formulation of the problem can be reduced to 
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three non-linear equations, one for X-coordinate, one for Y-coordinate 5 and one for heading 
angle \|/. A practical assumption is to optimize the path in the two dimensions X and Y only. 
The Z-coordinate of the path will then follow from the dynamic terrain model 821. The fol- 
lowing variables and equations are used: 

Xa = (Xa, Ya, Za) and 1|/a are coordinates and heading angle in the fixed to ground coor- 
dinate system 41 for vehicle 1, in switch point 1112 reconnaissance/approach path 
Xc = (Xc, Yc, Zc) and \|/c are coordinates and heading angle in the fixed to ground coor- 
dinate system 41 for vehicle 1 in loading 1221 respectively unloading point 1231. 

The three equations can be written 

7 

Xc - Xa = 2 x ( k X vectorial equation in 3D (12a) 
k= 1 
7 

- i|M = 2 voo ( 12b ) 

k = 1 

where the three dimensional vector X(k) and the angle \|/(k) constitute the additive contribution 
in coordinates X andY respectively heading angle \\f from each of the path segments No. k = 1 , 
2 3 7 

Each one of the two bends of the assembled curve, consists of one bending and one 
straightening clotoid segment and has to include , if the total heading angle change Cti in bend 
number "i" amounts to more than a determined amount do, one between the two clotoid seg- 
ments inserted cirkular segment with bending angle etc = Cti - Oto 

The vector klot(Mi,Si) is employed for the bending clotoid in bend No. i as a function of the 
parameter Si and the clockwise/counterclockwise factor Mi 
(clockwise, Mi= 1, counterclockwise, Mi = -1) 

u = Si u = Si 

klot(Mi, Si) = [! cos(u 2 /2) du, Mi J sin(u 2 /2) du] (13a) 
u=0 u=0 

and the vector cirk(Mi,otc) for the circular path as a function of its bending angle OU and Mi 

cirk(Mi,(Xc) = [sinac, Mi( 1 -cosotc)] (13b) 

where 

f V Oti if Oti < (Xo 

Si- < _ (13c) 

[ V CCo om oto < Cti 
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The following 3D coordinate transformation matrix is employed. 



r cosy* 


-sinv|/i 


ol 




1 sinyi 


COS\J/i 


ol 


(14) 


Lo 


0 


lj 





The contributions of each of the seven partial segments to the terms in equations (12a) and 
(12b) can now be stated. For the partial segments 1-3 and 5-7 the characters Mi respectively M2 
for their clockwise/counterclockwise factors and the characters ai, Si and ot2 5 S2 for angles and 
arguments for their clotoid vectors. 

Partial segment 1 . Bending clotoid in the first path bend where A = scale factor, common for 
the entire curve. 

X(l) = A klot(Mi, Si)*M(\|M) (15a) 

f0.5Mi-(Xi if ai < (Xo 
V|/(l)= \ (15b) 

lo.5Mra 0 if ao<ai 
Partial segment 2. Circular. Omitted if ai <ao 

X(2) = A-cirk(Mi, ai - ao )*M(l|/A+0.5Mi-a o ) (16a) 
V|/(2)= Mi-(ai-ao) (16b) 

Partial segment 3. Straightening clotoid. Obtained by means of in two orthogonal directions 
mirroring the vector for bending clotoid from the point where the path transfers from clotoid to 
straight path 

X(3) = A klot(-Mi, si)*M(l|/A+Mi ai) (17a) 

V|/(3)= V|/(l) (17b) 

Partial segment 4. Straight path, zero contribution to total heading angle increment. Segment 
travel length = LNGD- 

X(4) = (LNGD, 0)*M(vj/a+ Mi ai) (18a) 

\|/(4) = 0 (18b) 
Partial segment 5. Bending clotoid in the second path bend 

X(5) = A-klot(M2 5 S2)*M (Vj/A+ Mrai) (19a) 

f0.5M2-a2 ifot2<ao 
V|/(5) = i (19b) 

lo.5M2-ao if ao < a2 

Partial segment 6. Circular. Omitted if OL2 < OLo 

X(6) = A cirk(M2, 0L2 - a G )*M (\|/a+ Mi ai+0.5M2-a2) (20a) 
\(/(6) = M2*(CC2 - ao) (20b) 
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Partial segment 7. Straightening clotoid. Obtained by means of in two orthogonal directions 
mirroring the vector for bending clotoid at the endpoint of the total path where the heading 

angle is \\fc. 

X(7) = A klot( - M2, S2)*M(\|/c) (2 1 a) 

V|/(7)= V|/(5) (21b) 

The equation system (12), neglecting the Z-coordinate, is easily solved in the mission computer 
6 by means of standard mathematical procedures for non-linear equation systems and using ran- 
dom start solutions in the relevant variables Mi, M2, LNGD, Oti and Ct2. The resulting path can 
be used both for forwards and backwards driving. 

Loading operation optimisation based on the dynamic terrain model and a model of the mecha- 
nics of the load handling implement 

When the report point 7277 on the dynamic approach path has been attained and loading path 
message 982 from the DTM-computer 82 has been received by the mission computer 6, mes- 
sage data is used for optimizing parameters for vehicle and implement control in the dynamic 
loading path 722. For this purpose is used in the optimization, both the coefficients 9821 of 
ground plane at the vehicle loading point 7227 and the loading height profile data list 9822 with 
the table z(k), k=0,l,2,... for terrain height at successive points along the intended loading path 
when penetrating the material volume 757, cf FIG 13. Loading path depth and velocity, and 
bucket 142 lift and tilt movement parameters are optimized considering the requirements and 
constraints of filling the bucket, such as cost, minimising time of operation, minimising ground 
friction from bucket, minimising material spill and keeping power requirements below available 
power train capacity. 

Groundplane model for the vicinity of the loading point 

For optimising the bucket 142 movements in the dynamic loading path 722 it is needed, for any 
advancement of the vehicle 1 along the intended loading path to be able to estimate a six de- 
grees of freedom position in the fixed to ground coordinate system 41 of the fixed to vehicle 
coordinate system 42. 

As the bucket's 142 penetration of a material volume during loading normally is limited to a 
rather short advancement, typically 1 - 3 meters, and assuming that the grund surface is rela- 
tively flat, it can be taken that a simple plane surface model for the vehicle's position in the 
loading path will do, and that basing the model on this assumption will not contribute to more 
than marginal errors in comparison with the real ground surface. A linear equation for the gro- 
und plane model in immediate vicinity of the loading point 7227 with coordinates (Xc, Yc) can 
be written as: 

X*Xn + Y-Yn + ZZn = C (22) 

An estimation of the coefficients 9821 Xn, Yn and Zn and the constant C can be obtained by 
means of the standard method of least squares with minimum 5 representative elements in the 
DTM 821 for a number of points on the current best model estimation Z(3, n) of total area ter- 
rain surface in a vicinity of the vehicle loading point 7227. In the actual application, it is no 
difficulty to get a sufficient number of measurements for the reference ground surface in such 
an area. The ground plane model Eq (22) coefficient estimations are performed dynamically in 
the DTM-computer 82 when the vehicle 1 is on its way to the report point 7277 on the dynamic 
approach path 727, where the loading path message 982 based on the actual terrain model Z(l, 
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n) , n=l,2,... from the vehicle's current run is sent to the mission computer 6 for its optimization 
of the parameters 9312 for the dynamic loading path 122. The mission computer then employs 
these coefficients 9821 from the message 982 for obtaining, for the loading path, the vehicle's 
expected heading, pitch and roll angles i|/c, dc, and cpc, respectively, specifically used as angular 
arguments of a transformation matrix M(ij/c, Sc, cpc) needed for the loading process optimiza- 
tion, where Eq (3) is used to convert coordinates in the fixed to vehicle coordinate system 42 to 
coordinates in the fixed to ground coordinate system 41. To obtain these angular arguments, the 
following model, in Eqs (23a), (23b), (23c) comprises the 3D vector position X(s) for a path 
from the loading point 1221 as a function of vehicle advancement s in the intended direction i|/ c 
and Eqs (24a), (24b), and (24c) for the corresoponding position X(r) at a lateral movement r in a 
perpendicular direction to the right on the same surface: 



Inserting (23a), (23b) and (23c) in Eq (22) gives a solution in Eq (25a) below for the angle »c. 
Likewise with (24a), (24b) and (24c) gives Eq (25b) below for the angle cpc. 



Estimated loading height profile from the attack point 1222 

Optimizing the parameters 9312 for the dynamic loading path 122 entails the parameters loa- 
ding path depth, vehicle velocity and bucket 142 lift and tilt movement parameters. The optimi- 
zation approach chosen in this invention is to employ the developing terrain model Z(l, n) from 
the vehicle's current running in conjunction with an actual best estimation Z(2, n) of the ground 
surface in order to be able to predict the outcome of various possible parameters for steering the 
vehicle and its implements. A principal idea is to keep the total model in the DTM computer 82, 
and only to send over relevant information for the imminent needs of the mission computer 6. 
In practise, a two-dimensional height profile model of the terrain surface from the attack point 
1222 along a vertical section through the linear loading path in the x/y-plane has been found to 
convey sufficient information for the optimization needs. 

This loading height profile Zioad = Zioad(Sg) data list 9822 is such a model. It is approximated, cf 
FIG 13, from layer 1 numbers Z(l,n), elements n belonging to a subarea of DTM 821 involving 
a sufficient part of the pertinent material volume 181. The height profile table Zioad = Zioad(Sg) 
represents an average of estimated terrain z-coordinates in the fixed to ground coordinate sys- 
tem 41, in a set of elements {nij, j= 1,2,3,... jmax} inside a rectangular area centered around the 
forward edge of the bucket 142, this edge being on horisontal distance Sg = i d from the attack 
point 1222 for bucket's entry in the material volume at loading operation, and d is a suitable 
sampling distance between consecutive Sg numbers: 



X(r) 



X(s) 



fX(s) = Xc + S COS(V|/c) 
\ Y(s) = Yc + s sin(v|/ c ) 
I Z(s) = Zc + s tan(»c) 
f X(r) = Xc - r sin(\|/c) 
\ Y(r) = Yc + r cos(\|/c) 
I Z(r) = Zc - r tan(cpc) 



(23a) 

(23b) 

(23c) 

(24a) 

(24b 

(24c) 



ftan(Sc) = - [Xn cos(\|/c) + YN*sin(\(/c)]/ Zn 
ltan(<p c ) = [XN-sin(i|/c) - Yncos(i|/c)]/ Zn 



(25b) 



(25a) 



j max 
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Determining the bucket 142 position in the fixed to ground coordinate system 41 
In a loading movement the bucket 142 starts with its blade above but close to ground and penet- 
rates the material volume 181, at the same time as hydraulic pressure is increased on the lift 
cylinders 1411 in order to reduce ground friction from the weight of the bucket and its collected 
load. When a finishing lift can be expected to result in a full bucket, the penetration is halted. 
The loading movement concludes with the final lift of the bucket to get clear from the remai- 
ning material when returning with the load. A principal idea of this invention, in addition to 
having a good model of the material volume along the loading path, is to be able to control the 
bucket accurately during this movement by employing an accurate model of the bucket's geo- 
metry in a fixed to ground system as a function of vehicle advancement from vehicle loading 
point, and as a function of lift and tilt actuation of the load handling implement 14, as governed 
by a loading process sequential step variable k = 0, 1, 2, .... for controlling with good coordina- 
tion each of these movements and actuations. It is also important to be able to predict the accu- 
mulated volume and weight of the pieces of material cut out from the material volume by the 
bucket, as well as the available bucket volume, in the various steps of a candidate loading pro- 
cess. 

FIG 12 shows in a section the mechanics of the load handling implement 141 and the bucket 
142 and frontal parts (one wheel and part of vehicle framework) from a loading vehicle. 

For a number of progressive loading loading process states k k = 0,1,2,..., the position of the ^ 
bucket 142 can be predicted as the coordinates X(k) = [x(k), y(k), z(k)] in a fixed to ground 
coordinate system 41 for a number of points on the bucket geometry. 

The geometry of the vehicle is in a first step determined in the fixed to vehicle coordinate sys- 
tem 42. In a proceeding step these local coordinates [^(k), T](k), £(k)] are transformed to a fixed 
to ground coordinate system 41 by the following standard transformation equation. 

X(k) = X(s) +K(k), ri(k), «k)r M(i|/c, Sc, cpc), k = 0,1,2,..., (27) 

For Eq (27), it is assumed that the origo of the fixed to vehicle coordinate system is located on 
ground level. The transformation matrix with the arguments i|/c, Jk and cpc can be determined as: 

M(\|/c, Sc, cpc) = 

|~cos3c cosyc cosOcsinyc sin&c ] 

= I - coscpc sinvj/c + sincpc sin$c cosyc coscpc cosv|/c! + sincpc sindc sinij/c - sincpc cos&c |... (28) 
L- sincpc sinv|/c - coscpc sinSc cosvj/c sincpc cosvj/c - coscpc sin$c sinipc coscpc cos&c J 

The steering angle is assumed to be zero and the local coordinate r| for each point in the load 
handling implement can accordingly be considered to be invariant during the loading process. 
In order to obtain, as a function of loading process state k, the for the above transformation 
required local coordinates §(k), r|(k) and c^(k) for points on the mechanical structure of the load 
handling implement 14, this structure can be considered as a number of partly linked rigid 
mechanical elements one being the bucket 142 itself and where a hydraulic cylinder can be mo- 
deled by a pair of rigid and both limited to move along a common straight line through two cy- 
linder attachment pivots. In order to obtain how extending or contracting the cylinders will 
change the position of the bucket and assuming zero steering angle the mechanical structure can 
be quite accurately approximated by a structure in two dimensions, % (forward) and £ (upward), 
in the fixed to vehicle coordinate system 42. The rigid elements, consisting of planar bars and 
rods and the bucket represented by a polygon, are linked together by means of a number pivot 
axles perpendicular to the c^-plan. Each pivot or polygon point can be represented by a unique 
integer from i = 1 to i = imax. Cf simplified model, FIG 13. 
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For each element j e {El, E2, E4} belongs a number of such points which, due to the 
rigidity of the element and independent of the position of the element have constant 
distance to all other points on the same element. Locally, within the set of points be- 
longing to the rigid element j these points can be represented by integers m = 0, 1,2,..., 
mmax(j). Specifically, m = 0 can be chosen to represent the center of gravity for an ele- 
lent and mmax(j) is the total number of pivot and polygon points in element j. 

For the abovementioned points the t/t^ coordinates can be written as 
f^(j,m, k) = £ -coordinate, loading process state k for 
| point m in the rigid element j (29a) 

I £G,m, k) = £ -coordinate, loading process state k for 
I point m in the rigid element j (29b) 

Each rigid element El, E2, E8 is required to have, for defining the element's position and 
orientation, at least one pivot point and one additional index point. The latter must not necces- 
sarily be a pivot point. For these two primary points we define 

Pivot point, order number m = 1 
Indexpoint, order number m = 2 

The table below shows a choice of pivot point, index and remaining points for the simplified 
load handling implementet mechanichal structure 141 model. Cf FIG 12. The center of gravity 
points are not shown in the drawing and the bucket geometry defining points are not listed in 
the table. 
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Lift cylinder, part 2 
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Tilt cylinder, part 1 
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Tilt cylinder, part 2 
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In the two dimensional model of the mechanics 141 of the load handling implement the coordi- 
nates of the pivot and index points as functions of the extensions of the lift and tilt cylinders can 
now be determined by means of classic analytic plan geometry 



The coordinates of the bucket 142 geometry position and orientation defining points can be 
determined in the same way as the already treated pivot and other points. Bu specifically 
emploing a number of points on the inside of the bucket, the coordinates of these points can be 
used for determining the volume loading capacity of the bucket for any state k of the penetra- 
tion and loading process. The bucket defining points can be represented by an integer number 
sequence of msmax numbers included among the total set of numbers representing the rigid ele- 
ment E4 in the simplified model. Thus for bucket geometry defining points: 

Ills = 1, 2,..., msmax (30) 

Employing Eq (27), the bucket defining points for each state k of the penetration and loading 
process can now be determined in the fixed to ground coordinate system 41 by the coordinate 
vector X(4, m s , k): 
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X(4, m s , k) = X(s) +K(4, ms, k), ti(4, m s , k), £(4, ms, k)]* 

* M(vj/c 5 Oc,(p c ) (31) 

As all bucket defining points can be placed in the - plane of the fixed to vehicle coordinate 
system 42 , all r|(4, m s , k) can be set to zero in Eq (31) above. 

Estimation of the volume loaded, for a given vehicle 1 and load handling implement 14 loading 
path and movement 

For optimizing, in any loading cycle, vehicle 1 and load handling implement 14 loading path 
and movement, it is needed to have a procedure and software for estimation of the volume loa- 
ded for any given such operation. The estimation can be based on the loading height profile 
data list 9822 which, for successive depths s g = i d, i = 0,1,2,... of penetration, as counted from 
the attack point 1222, represents an estimated height profile Zioad(i), i = 0,1,2,... in fixed to gro- 
und coordinates of the material volume , as outlined above, Cf Eq (26). In a certain state k of 
the loading movement the edge of the bucket 142, can be expected to be located on a rather 
well-defined position inside the material volume. At this stage, a certain volume amount of the 
material volume then has been cut out by the forward edge of the bucket and been captured by 
the bucket. The volume amount thus cut out and captured depends on the novements and shape 
of the bucket and on the shape and properties of the material being loaded. Under the condition 
that the material being loaded is a relatively easy-running solid material, like sand, gravel or 
sufficiently broken down stone or other materials, the volume cut out and captured can be esti- 
mated with sufficient accuracy for each step of the loading process as a basis for an effective 
optimization of this process. 

Let AVs(k+l) be the volume cut out between the states k and k+1 of the loading process. Assu- 
me also that, in these states, the bucket 142 has not penetrated too deep in the material volume 
181. Consider a vertical cut of the material volume and oriented along the planned loading path. 
The near trapezoidal polygon "abed" in FIG 14 represents a projection on this cut of the expec- 
ted volume being cut out between those two states, k and k+1 of the loading process . The upper 
edge "be" of the polygon is part of a polygon through or approximating the sample points from 
the loading height profile data 9822. The lower edge "da" of the polygon is part of a polygon 
through the bucket edge in its sequential s/z plane positions according to Eq (31) [s g (k), z(4, m g , 
k)], k=0,l,2,... in the fixed to ground coordinate system 41. Assuming that the bucket 142 cross- 
section is constant over its entire width B and that the polygon can be approximated by the tra- 
pezoid "abed" the following equation holds for AVs(k+l): 

AVs(k+l) = V 2 Bh [Ziappr(k) + Zia PP r(k+l) - z(4, m g , k) - z(4, m g , k+1)] (32) 

where 

h = Sg(k+l)-s g (k) (33) 
and Zia PP r(k) = [s g (k) - i d )/h]-Zioad(i) + { 1 - [s g (k) - id )/h]}-Zioad(i+l) (34) 
where 

i-d < s g (k)<(i+l)-d (35) 

and 

s g (k) = V[x(4, mg, k) - Xatac] 2 + [y(4, m g , k) - Yatac] 2 (36) 

Let Vs(k) be the total accumulated volume amount cut-out, at loading process state k of the lo- 
ading process, by the bucket's edge from the material volume: 
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f r=k 

I Vs(k) =ZAVs(r) 5 k=l, 2, 3 5 ... (37a) 

i r=l 

I 

I Vs(0) = 0 (37b) 

The bucket's 142 volume holding capacity depending on its orientation, cf FIG 15, can be esti- 
mated from the bucket geometry and the properties of the loaded material. In an effective loa- 
ding movement the flat lower plate of the bucket is initally driven into the material volume very 
close to the ground. During the corresponding states of the loading process it is important, in 
order to reduce or eliminate bucket's ground friction, to allow for sufficient hydraulic pressure 
in the lift cylinder circuits in order to balance the empty weight of the bucket and the rest of the 
load handling implement mechanical structure. At the same time, the bucket should not be al- 
lowed to rise or tilt significantly which might force a premature abortion of the penetration. 
Gradually, as the penetration continues, bucket load from the received material is increasing. At 
that stage, support reaction forces might increase acting on the bucket and causing friction. By 
comparing the hydraulic pressure in the lift cylinder circuits with a desired pressure considering 
also the expected weight of the loaded material at state k, the hydraulic pressure can be adjusted 
in order to facilitate the penetration by reducing friction due to support reaction forces to a mi- 
nimum. Still it is importanbt to keep the bucket low and not tilted. In this way the penetration 
can normaly continue until the volume cut out equals the instantaneous volume holding capaci- 
ty of the bucket. At this stage the penetration stops, but hydraulic pressure on the lift cylinder 
can now be allowed to increase further in order to lift the bucket. At the same time the tilt cyli- 
ders are engaged in order to increase the bucket holding capacity to its maximum before the 
bucket will raise above the upper edge of the material volume. 

Bucket lift can continue until the bucket is free from the material volume. At this moment the 
return movement on a dynamic transport path 124 can be initiated, beginning during the first 
piece of the path by lowering the bucket to a low center of gravity transport position. In certain 
cases the conditions at penetration are such that the bucket starts to rise markedly before the 
desired bucket pressure can be reached. In this case the material density is probably lower than 
expected. The density parameter might then have to be changed. On the other hand if the mate- 
rial density is significantly higher than expected, the intentionally increased hydraulic pressure 
might not be sufficient in order to reduce friction causing slipping which also might lead to an 
unsuccessful loading attempt. 

Estimation of required lift force and power during the loading path 

The estimation is based on knowing, with reasonable accuracy, and for each state of the loading 
process, in the fixed to ground coordinate system 41, the z-coordinate and mass of each of the 
moving elements. By knowing this, the potential energy of the system consisting of these mo- 
ving masses can also be estimated. We can also assume that the vehicle is moving on a flat but 
not neccessarily horisontal surface according to Eq (22). With zero friction, the dynamic pro- 
cess of the loading movement consumes, in any time interval, an amount of energy equal to the 
work needed for lifting each of these masses. By differentiating the potential energy for each of 
these masses as a function of the state variable k, an estimation can be made of lower bound 
variables for the required lift forces and power needs. 

Loadingsrorelsens dynamic forlopp medfor darvid under varje tidsintervall ett arbete som ar 
minst lika med summan av det arbete som erfordras for att lyfta upp var and en av dessa mas- 
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sor. Genom att stalla upp ett uttryck for den potentiella energin for var and en av dessa massor 
som function av tillstandsvariabeln k kan darur erforderliga lyftkrafter and efFektbehov under en 
loadingsrorelse beraknas 

As shown above, for each state k of the loading sequence, the 3D position, in a coordinate sys- 
tem fixed to ground, of the main elements of the load handling implement including the bucket 
can be estimated with fair accuracy. This also applies to each element's center of gravity. 
As the shape of the bucket is known and by using the simple assumption that the upper surface 
of the volume cut out by and received in the bucket is flat and horisontal, also the fixed to gro- 
und coordinates of the center of gravity of the accumulated load in the bucket can be determi- 
ned with reasonable accuracy. Obviously, also the weight of all elements of the load handling 
implement are known and we have shown above in Eq (37) how the cut out and accumulated 
loaded volume Vs(k) can be estimated. By using an estimate of the material density, Vs(k) can 
be transformed to a weight estimation of the load in the bucket. 

An expression for the potential energy at each sequential state of a loading process 
Exploiting the symmetry of the load handling implement 14 and bucket 142 on both sides of the 
^-plane in the fixed to vehicle coordinate system, and assuming small roll angles and roll 
angle motions, the two dimensional model of the load handling implement 14 and bucket 142 
and the above explained concepts can also be employed for estimating volumes, weights and 
centers of gravity. 

For state k of the loading process, let Umek(k) be the potential energy of the mechanical ele- 
ments of the load handling implement 14 with bucket 142, and let Us(k) be the potential energy 
of its received and accumulated load. Let U(k) be the total potential energy in state k: 

U(k) = Umek(k) + Us(k) (38) 

The control dynamics allows for the load handling implement 14 to be controlled from a 
sequential instruction flow with constant sample time TSAMP and thus tk+i - tk = TSAMP in- 
variant of k for the sampling instances t = ti, t2, t3, ... .Let k(tk) be the desired state at t = tk and 
let AU(k) be the work required to bring the system from state k to state k+1 : 

AU(k) = U(k+l)-U(k) (39) 

A complete loading sequence typically requires some ten seconds from attack of material volu- 
me to ready for return movement, while the sampling time TSAMP ought to be in the order of 
some 0.1 s or less in order to keep the system within normally acceptable error margins. Consi- 
dering unavoidable model errors due to approximations regarding material density and loaded 
volume shape, effects of kinetic energies not considered in the models etc, we can assume the 
power needed to be constant during each time interval [tk, tk+i], k=0,l,2,3,... Let P(k) be the 
average power needed during such a time interval, and our assumption can be formulated as: 

P(k) - AU(k) /TSAMP (40) 

Eq (40) can be used for optimizing lift and tilt movement speeds in order to minimise total time 
for the loading process within the constraint of not exceeding available implement actuation 
power. 

An expression for Umek(k) 

Let MQ) be the mass of the rigid element Ej and let Umek(k) be the potential energy, in a fixed to 
ground coordinate system 41, of the load handling implement's 14 mechanics 141 and bucket 
142, but excluding the load cut out and received by the bucket: 
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j max 



Umek(k) = XgMG)[z(j, 0 5 k)- Z (j, 0, 0)] 



(41) 



where g is the vertical acceleration of gravity 
An expression for Us(k) 

Let Us(k) be the potential energy, in a fixed to ground coordinate system 41, of the load cut out 
and received by the bucket. Assume further that the center of gravity for the load is close to the 
center of gravity for the bucket: 



Where p is a measure of density representing the loaded material, in kg/m 3 
Estimating the magnitude of the support reaction forces on bucket 

When the bucket 142 is supported by ground and has to be moved laterally, a friction force ac- 
ting on the bucket and oriented as to repell the movement is excited. The strength of this fric- 
tion force is proportional to the support reaction forces on bucket. By estimating the magnitude 
of this force F(k) as a function of loading state k, it is feasible to control the hydraulic pressure 
in the lift cylinder of the load handling implement in order to minimise this support reaction 
force and load movement repelling friction forces and instead to increase the reaction forces on 
the front wheels of the loading vehicle in order to improve traction and reduce the risks of slip- 
ping wheels. 

The work required to work against the support reaction force an infinitesimal distance h equals 
the work required to lift the load handling implement with bucket and load the same distance h. 
Provided that the latter work can be estimated, from this equivalence the support reaction force 
F(k), k= 1, 2, 3, ... can be obtained. 

Let F(k) be the support reaction force in a state k. The work required to move against the sup- 
port reaction force an infinitesimal distance h can then be expressed as F(k)-h. 

Let Az(j,0,k) + Oj(h) be the positive vertical movement in the fixed to ground coordinate system 
41 required for the center of gravity of element j when the bucket itself is subjected to apositive 
vertical movement h, corresponding to a work amount [Az(j,0,k) + Oj(h)]-gM(j) for each ele- 
ment j. For the bucket, assuming as above that the center of gravity for the load is close to the 
center of gravity for the bucket, the corresponding work amount can be estimated bythe expres- 
sion: p-g [Az(4,0,k) + Oj(h)]-Vs(k). 

From the above reasoning, F(k) can be solved starting from the following difference equation as 
shown for the simplified load handling implement mechanical structure of the example in FIG 
12, with element j=4 being the front end rod with the bucket fixed to it: 



Us(k) = p-g [2(4, 0, k) - z(4, 0, 0)]V s (k) 



(42) 



jmax 



where 



F(k)h = g X[AzGAk) + Oj(h)] M(j) + p-g [Az(4,0,k) + Oj(h)]Vs(k) 
: lim Oj(h) = 0- for h — > 0, 



(43) 
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and, introducing the geometry derivatives [dz(j, 0, ko)/dz(4, 0, ko)]; j = l,...jmax, Eq (44) re- 
sults: 

jmax 

F(k) = gZ M(j) [dzG ? 0, ko)/dz(4, 0, ko)] + pg Vs(k) (44) 
j=l 

Balancing the gravity of the load handling implement 14 during the loading process 
As mentioned above, friction forces can be significantly diminished if the support reaction for- 
ces on the bucket can be reduced by balancing the load handling implement through proper 
application of hydraulic pressure in the lift cylinders. Let Fi(k) be the force, in state k of the 
loading process, required to be exterted by the lift cylinder in order to balance the load handling 
implement including the expected gravity p g Vs(k) cut out and received by the bucket. Assu- 
ming that the mechanical structure of the load handling implement can be considered stiff so 
that the work delivered by the lift cylinder can be delivered practically with no loss to relieve 
the support force F(k) as obtained from Eq (44), the following work equation (45) can be appli- 
ed to an infinitesimal lift cylinder marginal extension Al and the corresponding marginal mo- 
vement h in the z-direction of the bucket 142, 

Fi(k)Al = F(k)h + o(h) (45) 

and by differentiation: 

Fi(k) = F(k)[dz(4,0,k)/dl] (46) 

From this force estimate, a corresponding hydraulic pressure can be derived knowing the area of 
the lift cylinder pistons. By including, in the vehicle and implement control data list 971, de- 
manded lift cylinder pressure as derived from Eq (46), friction forces from support reaction can 
be substantially reduced and maximum weight transferred to the front driving wheels of the 
vehicle 1 to facilitate the part of the loading process where the bucket penetrates the material 
volume 181. 

Obstacle detection and avoidance 

Realiable and accurate detection functions are needed for the safety of the environment, for 
human safety and for protecting the vehicle itself against accidents and damage due to collis- 
sions or maneouvers out of controlled paths, zones and enclosures. A system for obstacle de- 
tection requires sensors for detecting and recording occurring obstacles, at least their shape and 
position. This data must then be stored in a systematic manner, for allowing analysis and war- 
ning or emergency action based on the currently relevant collected obstacle data as well as re- 
garding the vehicle's 1 actual shape and current and its nearest planned or predicted continued 
path. 

Primarily an object of a certain minimum size such as height appearing inside an obstacle free 
zone 191 as defined in the DTM 821 can be considered to be an obstacle. It can also be the pur- 
pose of an obstacle detection system to check the vehicle's actual position and its path so that it 
does not, neither is predicted to, enter any area not inside any obstacle free, loading or unloa- 
ding zone. 

From a point the vehicle 1 can be associated with a number of zones in a fixed to vehicle co- 
ordinate system. A first such zone can be the nearest environment around the vehicle where 
detection and occurrence of an obstacle requires immediate emergency stop. A second such 
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zone can be an additional zone around the vehicle, but outside the first zone. Detection and oc- 
currence of an obstacle in this second zone might only require a warning or/and a slowdown 
action. The size and number of such zones depends on the vehicle's speed, maneouvering ca- 
pability, braking distance and the range of the obstacle detection system. Data regarding these 
zones can be conveyed in the mission instructions 9 in the form of zone, inner and outer, bor- 
der polygon list 9521 and 9522, respectively. During operation, the DTM-computer 82 can 
emloy this data together with from mission computer 6 obtained vehicle and implement control 
data list 971 for evaluating criteria based on data in the DTM 821 for warning or emergency 
action at occurrence of obstacle inside zone both related to the current position of the vehicle 
as its planned and predicted path according to the currentt vehicle and implement control data 
list. 

In preparation of each path, both the vehicle control computer 211 and the DTM-computer 82 
receive a vehicle and implement control data list 971. In the DTM-computer an obstacle avoi- 
dance mapping 1955 in fixed to ground coordinates can be produced from projecting the fixed 
to vehicle obstacle avoidance zones on the fixed to ground coordinate system 41 for each posi- 
tion of the vehicle in its imminent path according to the vehicle and implement control data list 
971. This obstacle avoidance mapping J 955 can be considered as a set of elements in the DTM 
821 being the union set of all elements occupied by some area from any of the projected vehicle 
obstacle avoidance zones on the fixed to ground coordinate system. Se FIG 18. This enables an 
inital check before starting the vehicle in the path that this path will keep the vehicle well inside 
the allowed obstacle free, loading and unloading zones. When the vehicle is underway on its 
path, the DTM computer constantly checks if any obstacle appears at any element of the DTM 
which is covered by any area from any of the vehicle's obstacle avoidance zones as projected on 
the fixed to ground coordinate system for the actual position of the vehicle. 

Let P(K, L) be points and let X(K 9 L) = fc(K, L), r|(K, L), £(K,L)] be the corresponding coor- 
dinate vectors in a three dimension coordinate table of the fixed to vehicle coordinate system 42 
where the points P(K, L) represent the vehicle's obstacle avoidance geometry 195 and for each 
of the levels L = 1, 2,..., LMAX consists of a fixed to vehicle obstacle avoidance zone J^CL), 
1951, contained within a closed border polygon co(L) 1952 with its corners being the points 
P(K, L) for K = 1 , 2, 3, KMAX. 

Let X(s) = [X(s), Y(s), Z(s)] be coordinates and vj/(s), 9(s), q>(s) be heading, pitch and roll ang- 
les in the fixed to ground coordinate system 41 for the fixed to vehicle coordinate system 42 in 
a point P(s) located on a distance s - so along the vehicle's planned path from its current posi- 
tion with its coordinates X(so), where this vehicle path is defined by a vehicle and implement 
control data list 971 from the mission computer 6. Let A(L, u) be an obstacle avoidance zone 
projection 1954 on level L of the fixed to vehicle obstacle avoidance zone JA{IS) on the DTM 
821 in the fixed to ground coordinate system, where this zone A(L, u) is contained within a clo- 
sed border polygon Q(L,u), 1953, as defined by points P(K, L, u), and their corresponding co- 
ordinate vectors X(K, L, u) = [X(K, L, u), Y(K, L, u), Z(K, L, u)], K=l, 2, KMAX and whe- 
re each such point P(K, L, u) is a projection on the DTM in det fixed to ground coordinate sys- 
tem 41 of the point p(K, L) with the coordinate vector X(K, L) = [£(K, L), ti(K, L), ^(K,L)] 
belonging to the border polygon o>(L) in the fixed to vehicle coordinate system 42. Finally, a 
fixed to ground obstacle avoidance mapping 1955 is made up as the union set 0(L,s) of all ob- 
stacle avoidance zone projections A(L, u) from the vehicle's different positions "u" along its 
path, so < u < s. 

Four steps to obtain the fixed to ground obstacle avoidance mapping 1955: 
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1° Let P(K, L, u) be the projection on DTM 821 in fixed to ground coordianet system 41 of the 
3D coordinate point P(K, L) in the fixed to vehicle coordinate system 42. Let P(K, L, u) have 
the coordinate vector X(K, L, u) = X(K, L, u), Y(K, L, u), and Z(K, L, u)]. This vector can br 
obtained from Eq (50) below, where X(u), Y(u) and y(u) are, in the fixed to ground coordi- 
nate systeme 41 , 2D position coordinates and heading angle, respectively, for the fixed to 
ground coordinate system 42 according to vehicle and implement control data list 971: 

X(K, L, u) = X(u) + X(K, L)*M[i|/(u), 9(u), cp(u)] (50) 

the matrix M is defined in Eq (2) and the pitch and roll angles can be estimated from the 
DTM from the Z-coordinates of a number of elements in DTM around the element occupied 
by [X(u), Y(u)] and by using a standard least squares estimation like the one employed for 
finding Eq (22) coefficients, Xn and Yn. From thus estimated Xn and Yn for the point on 
[X(u), Y(u)], 9(u) and cp(u) can be obtained from the equations (25a) and (25b). 
The Z-coordinate can be obtained directly from Eq (22) as Z(u) = [C(u) - X(u) Xn(u) - Y(u) 
Yn(u)]/ Zn(u). 

2° The polygon Q(L,s) is made up by X and Y coordinate points from the coordinate vector 
X(K, L, u), K=l, 2, KM AX. 

3° The obstacle avoidance zone projection 1954 A(L,u), is the surface within the closed po- 
lygon Q(L,u). Elements of the DTM with some area inside the Q(L,u) are considered to be- 
long to this zone projection. 

4° A fixed to ground obstacle avoidance mapping 1955 is represented by the union set 0(L,s): 
u = s 

0(L,s) = U A(L,u) (51) 

U = So 

The obstacle avoidance assignment of the DTM-computer 82 is, with a predetermined rate to 
analyse the DTM 821 inside areas 0(L,s), L=1,...,LMAX, and carry out those actions possibly 
following the obstacle avoidance analysis according to the criteria listed below. In addition, for 
each new vehicle and implement control data list 971 the DTM has to evaluate it according to 
the criteria below for possible conflicts with established obstacle free, loading and unloading 
zones. 

1° If, for any element in DTM Z(4, n) = 1 and this element is also within 0(L,s o ), but given 
threshold value H is exceeded by H < Z(l , n) - Z(2, n) for this element, then there is at least 
one element of DTM within level number L of the fixed to ground obstacle avoidance map- 
ping 1 955 that has to be considered as an obstacle requiring obstacle avoidance action with 
message number H(0, L). 

2° If, for any element in DTM Z(4, n) ,Z(5, n) and Z(6 n) all are zero and if this element also is 
within 0(L,so), at least one point of the fixed to ground obstacle avoidance mapping 1955 
number L is not located inside any of the obstacle free, loading or unloading zones. This 
event requires obstacle avoidance action with message number H(0, L). 

3° If, for any element in DTM Z(4, n) - 1 and this element is also within 0(L,so), but given 
threshold value H is exceeded by H < Z(l , n) - Z(2, n) for this element, then there is at least 
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one element of DTM within level number L of the fixed to ground obstacle avoidance map- 
ping 1 955 that has to be considered as an obstacle in the planned path requiring obstacle 
avoidance action with message number H(l, L). 
4° If for any element in DTM Z(4, n) ,Z(5, n) and Z(6 n) all are zero and if this element also is 
within 0(L,s), at least one point of the fixed to ground obstacle avoidance mapping 1955 
number L is not located inside any of the obstacle free, loading or unloading zones. This 
event has to be considered as a cause for rejecting the planned path requiring obstacle avoi- 
dance action with message number H( 1 , L). 

Obstacle avoidance messages from the DTM-computer at an inner obstacle avoidance geometry 
J-l and an outer obstacle avoidance geometry J=2: 

H(0, 1): Emergency stop message 9841 to the vehicle control computer 211 

H(0, 2): Warning message 9842 to the vehicle control computer 211 

H(l, 1): Rejection of planned path in revision message 980 to the mission computer 6 

H(l, 2): Warning message 9842 to the vehicle control computer 211 

Other aspects, objects and advantages of the present invention can be obtained from a study of 
the drawings, the disclosure and the appended claims. 



